package OperatorStation;

// PID class to be written by you
public class PID {

  // Private attribute containing a reference to the GUI
//  private OpCom GUI;
  // Private attribute containing a reference to the PIDParameters
  // currently used
  private PIDParameters p;

  // Additional attributes
  	private double ad,bd,yold,y;
	private double error,v,P,I,D;

  // Constructor
  public PID(String name){
    	p = new PIDParameters();
  	p.K=-0.2;//-0.2->-0.01	//-0.2
	p.Ti=0;			// 2.0
	p.Tr=10;		//10
	p.Td=0.5;//0.5->0.4	//1
	p.Beta=1;		//0.6
	p.H=0.1; //0.02->0.1	//0.1
	p.N=5; //5->10		//5
	p.integratorOn=false;
	setParameters(p);
	
//	GUI = new OpCom(this, p, name);
	
	P = 1; 
	I = 0;
	D = 0;
  }

  // Calculates the control signal v. Called from BeamAndBallRegul.
  public synchronized double calculateOutput(double y, double yref){
  	this.y=y;
    	error=yref-y;
  	P = p.K*(p.Beta*yref-y);
  	D = ad*D - bd*(y-yold);
  	return P+I+D;  
  }

  // Updates the controller state. Should use tracking-based anti-windup
  // Called from BeamAndBallRegul.
  public synchronized void updateState(double u){
    	if(p.integratorOn==true){
  		I=I+(p.K*p.H/p.Ti)*error+(p.H/p.Tr)*(u-v);
  	}
  	else{
  		I=0.0; 
  	}
  	yold=y;
  }

  // Returns the sampling interval expressed as a long. Explicit type casting
  // needed
  public synchronized long getHMillis(){
    	return (long)(p.H*1000.0);
  }

  // Sets the PIDParameters. Called from PIDGUI. Must clone newParameters.
  public synchronized void setParameters(PIDParameters newParameters){
  	p = (PIDParameters)newParameters.clone();
  	if(!p.integratorOn){
  		I=0.0; 
  	}
  	ad = p.Td/(p.Td+p.N*p.H);
  	bd = p.K*ad*p.N;
  }
  // Sets the I-part of the controller to 0.
  // For example needed when changing controller mode.
  public synchronized void reset(){
	I=0;
  }

  // Returns the current PIDParameters.
  public synchronized PIDParameters getParameters()
  {
	return p;
  }
}
